# for simulate map's set

import random,threading,math
import queue
from socket import *

from position import Position
from distance import Distance

import position_config as pc
import simulate_engine.calc_lab as calc_lab


SERVER_IP = '127.0.0.1'
SIM_PORT = 41997

class SimulateMap():
    def __init__(self,base=1):
        self._E = 2400*base
        self._base = base 
        self._walls = []

        # status info
        self.pos = None
        self.inter_raw_dis = None
        self.inter_md_dis = None
        self.inter_ps = None

        self.initialized = False
        self.random_map()
        self.initialized = True

        self.seg_list = []
        self.count_segment()

        self.sensor_info = queue.Queue()
        self.server_socket = socket(AF_INET, SOCK_DGRAM) 
        self.server_socket.setsockopt(SOL_SOCKET, SO_REUSEADDR, True)
        self.server_socket.bind((SERVER_IP, SIM_PORT))  
        self._reply_addr = None

        t=threading.Thread(target=self.send_msg,args=())
        t.daemon = True 
        t.start()   

        t=threading.Thread(target=self.recv_msg,args=())
        t.daemon = True 
        t.start()  

    def E(self):
        return self._E

    def count_segment(self):
        ps = [(0,0),(0,self._E),(self._E,self._E),(self._E,0)]
        for i in range(4):
            j = (i+1)%4
            self.seg_list.append((ps[i],ps[j]))

        for w in self._walls:
            x,y,xl,yl = w
            ps = [ (x,y),(x,y+yl),
                    (x+xl,y+yl),(x+xl,y)]
            for i in range(4):
                j = (i+1)%4
                self.seg_list.append((ps[i],ps[j]))
    
    def segments(self):
        return self.seg_list

    def random_map(self):
        # while True:
        #     # x = random.randint(0,self._E)
        #     # y = random.randint(0,self._E)
        #     x = 599
        #     y = 900
        #     if (self._legal_walls(x,y)):
        #         x_len = min(x,self._E-x)
        #         y_len = min(y,self._E-y)
        #         x_pos = 0 if x<self._E-x else x
        #         y_pos = 0 if y<self._E-y else y
        #         self._walls.append([x_pos,y_pos,x_len,y_len])
        #         break
        walls=[
            [0,6,3,2],
            [9,4,3,2]
        ]
        for w in walls:
            tmp = [x*200*self._base for x in w]
            self._walls.append(tmp)

    def _legal_walls(self,x,y):
        dis = [x,self._E-x,y,self._E-y]
        print(x,y,dis)
        mi_dis = min(dis)
        if (mi_dis>=200 and mi_dis<=400):
            return True
        return False

    def walls(self):
        return self._walls
    
    def send_msg(self):
        while True:
            dis = self.sensor_info.get()
            dis = "{}".format(dis)
            if (self._reply_addr):
                self.server_socket.sendto(dis.encode('utf-8'),self._reply_addr)

    def recv_msg(self):
        while True:
            try:
                recv_msg,recv_addr = self.server_socket.recvfrom(1024)
            except:
                continue

            recv_msg = recv_msg.decode('utf-8') 
            if (recv_msg.startswith('Initial')):
                self._reply_addr = recv_addr


    def update(self,irs_pos_dict,with_tag=False,recv_time = 0.0):
        # print('updata_pos ',irs_pos_dict)
        irs_pos = Position(irs_pos_dict,is_irs_center=True)
        calc_state,self.inter_raw_dis,self.inter_ps = calc_lab.sensor_distance_generate(irs_pos,self)
        
        if calc_state:
            tmp = Distance(self.inter_raw_dis,handle='recover').list_style[:]

            # FBLR
            tmp = [int(tmp[i]) for i in range(4)]
            print("sim_dis = ",tmp)

            if (tmp!=self.inter_md_dis):
                self.inter_md_dis = tmp[:]
                self.inter_md_dis.append(with_tag)
                self.inter_md_dis.append(recv_time)
            
                # print('put ',self.inter_md_dis)
                self.sensor_info.put(self.inter_md_dis)

            self.pos = irs_pos_dict

    def get_inter_info(self):
        status = True
        if not self.pos:
            status = False
        return status,self.inter_md_dis,self.inter_ps







